#include "imu.h"
#include "math.h"
#include "filter.h"
#include "mpu9250.h"
#include "AK8975.h"
#include "usart.h"
#include "ppm.h"
#include "led.h"
#include "control.h"
#include "IMUCailbration.h"
#include "AT24C02.h"
#include "delay.h"
#include "myMath.h"
#include "timer.h"
#define Acc_Gain  	0.0001220f				//加速度变成G (初始化加速度满量程-+4g LSBa = 2*4/65535.0)
#define Gyro_Gain 	0.0609756f				//角速度变成度 
#define Gyro_Gr	    0.0010641f			  //角速度变成弧度(3.1415/180 * LSBg)       

FLOAT_ANGLE Att_Angle,Att_Angle1;                       //飞机姿态数据
FLOAT_XYZ 	Gyr_rad;	              //把陀螺仪的各通道读出的数据，转换成弧度制
FLOAT_XYZ 	Acc_filt,Gry_filt;	  //滤波后的各通道数据
FLOAT_XYZ   acc_sample[6],acc_add,acc_off,acc_scale;
FLOAT_XYZ		gyr_add,gyr_off;
float   accb[3],DCMgb[3][3];                  //方向余弦阵（将 惯性坐标系 转化为 机体坐标系）
uint8_t AccbUpdate = 0;
float Gyr_Buffer[3],Acc_Buffer[6],Gyr_Off[3],Acc_Off[6];;
int16 Mag_off[3];
float acc_rol,acc_pit, gyr_rol, gyr_pit,gyr_yaw,Gyr_yaw,angle_f=0, angle_dot_f=0;
int imu_off=1,imu_off_flag=0;
float ag,ag_1,ag_off,agv,agx;
float Z_G,Control_Yaw;
/**************************实现函数*********************************************************************
函  数：static float invSqrt(float x) 
功　能: 快速计算 1/Sqrt(x) 	
参  数：要计算的值
返回值：结果
备  注：比普通Sqrt()函数要快四倍See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
*********************************************************************************************************/
static float invSqrt(float x) 
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}

/*********************************************************************************************************
*函  数：void IMU(void)(void)
*功　能：姿态解算
*参  数：无
*返回值：无
*备  注：此函数对原始数据进行校准（去零偏，滤波处理，赋予物理意义 并解算姿态
**********************************************************************************************************/	
void IMU(void)
{
	MPU9250_Read();    //触发读取 ，立即返回
	if(imu_off==1)
	{
	//加速度AD值 转换成 米/平方秒 
	Acc_filt.X = (float)MPU9250_ACC_RAW.X * Acc_Gain * G * acc_scale.X- acc_off.X ;
	Acc_filt.Y = (float)MPU9250_ACC_RAW.Y * Acc_Gain * G * acc_scale.Y- acc_off.Y ;
	Acc_filt.Z = (float)MPU9250_ACC_RAW.Z * Acc_Gain * G * acc_scale.Z- acc_off.Z ;
	
	//陀螺仪AD值 转换成 弧度/秒    
	Gyr_rad.X = (float) MPU9250_GYRO_RAW.X * Gyro_Gr - gyr_off.X ;  
	Gyr_rad.Y = (float) MPU9250_GYRO_RAW.Y * Gyro_Gr - gyr_off.Y ;
	Gyr_rad.Z = (float) MPU9250_GYRO_RAW.Z * Gyro_Gr - gyr_off.Z ;

	
   //陀螺仪解算姿态//
  //gyr_rol+=Gyr_rad.X*0.29f;
	//gyr_pit+=Gyr_rad.Y*0.29f;
	
	//加速度计解算姿态//
	//acc_rol=atan(Acc_filt.Y*invSqrt(Acc_filt.X*Acc_filt.X+Acc_filt.Z*Acc_filt.Z))*RadtoDeg;
	//acc_pit=-atan(Acc_filt.X*invSqrt(Acc_filt.Y*Acc_filt.Y+Acc_filt.Z*Acc_filt.Z))*RadtoDeg;
	
	IMUupdate(&Gyr_rad,&Acc_filt,&Att_Angle);//互补滤波
	//AHRSupdate(Gyr_rad.X, Gyr_rad.Y, Gyr_rad.Z,Acc_filt.X, Acc_filt.Y, Acc_filt.Z, Mag.X, Mag.Y,Mag.Z);
}
	else //传感器校正时
	{
		Acc_filt.X = (float)MPU9250_ACC_RAW.X * Acc_Gain * G ;
		Acc_filt.Y = (float)MPU9250_ACC_RAW.Y * Acc_Gain * G ;
		Acc_filt.Z = (float)MPU9250_ACC_RAW.Z * Acc_Gain * G ;
	
		//陀螺仪AD值 转换成 弧度/秒    
		Gyr_rad.X = (float) MPU9250_GYRO_RAW.X * Gyro_Gr ;  
		Gyr_rad.Y = (float) MPU9250_GYRO_RAW.Y * Gyro_Gr ;
		Gyr_rad.Z = (float) MPU9250_GYRO_RAW.Z * Gyro_Gr ;
	}
}

/*********************************************************************************************************
*函  数：void IMUupdate(void)
*功　能：互补滤波姿态解算
*参  数：无
*返回值：无
*备  注：无
**********************************************************************************************************/	
//kp=ki=0 就是完全相信陀螺仪
#define Kp 6.0f                         // 1.5 proportional gain governs rate of convergence to accelerometer/magnetometer
                                         //比例增益控制加速度计，磁力计的收敛速率
#define Ki 0.005f                        // integral gain governs rate of convergence of gyroscope biases  
                                         //积分增益控制陀螺偏差的收敛速度
#define halfT 0.0025f                     // half the sample period 采样周期的一半
float dif;
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;     // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error

void IMUupdate(FLOAT_XYZ *Gyr_rad,FLOAT_XYZ *Acc_filt,FLOAT_ANGLE *Att_Angle)
{
  float ax = Acc_filt->X,ay = Acc_filt->Y,az = Acc_filt->Z;
  float gx = Gyr_rad->X,gy = Gyr_rad->Y,gz = Gyr_rad->Z;
  float vx, vy, vz;
  float ex, ey, ez;
	float norm;

  float q0q0 = q0*q0;
  float q0q1 = q0*q1;
  float q0q2 = q0*q2;
  float q0q3 = q0*q3;
  float q1q1 = q1*q1;
  float q1q2 = q1*q2;
  float q1q3 = q1*q3;
  float q2q2 = q2*q2;
  float q2q3 = q2*q3;
  float q3q3 = q3*q3;
	
	if(ax*ay*az==0)
 		return;
	
  //加速度计测量的重力向量(机体坐标系) 
	norm = invSqrt(ax*ax + ay*ay + az*az); 
  ax = ax * norm;
  ay = ay * norm;
  az = az * norm;
//	printf("ax=%0.2f ay=%0.2f az=%0.2f\r\n",ax,ay,az);
 
	//地面系的重力加速度[0,0,1]换到机体坐标系 
  vx = 2*(q1q3 - q0q2);												
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3 ;
 // printf("vx=%0.2f vy=%0.2f vz=%0.2f\r\n",vx,vy,vz); 
	
	//测量的重力向量与转换后的重力向量做叉积求出向量间的误差 
  ex = (ay*vz - az*vy); //+ (my*wz - mz*wy);                     
  ey = (az*vx - ax*vz); //+ (mz*wx - mx*wz);
  ez = (ax*vy - ay*vx); //+ (mx*wy - my*wx);

  //用上面求出误差进行积分
  exInt = exInt + ex * Ki;								 
  eyInt = eyInt + ey * Ki;
  ezInt = ezInt + ez * Ki;

  //将误差PI后补偿到陀螺仪
  gx = gx + Kp*ex + exInt;					   		  	
  gy = gy + Kp*ey + eyInt;
  gz = gz + Kp*ez + ezInt;//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

  //四元素的微分方程
  q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

  //单位化四元数 
  norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 = q0 * norm;
  q1 = q1 * norm;
  q2 = q2 * norm;  
  q3 = q3 * norm;
	
  //四元数转换成欧拉角(Z->Y->X) 
	
	////////偏航角解算-陀螺仪磁力计协同融合////////////
	yijiehubu_Gry(Mag_yaw, Gyr_rad->Z,&Att_Angle->yaw);//偏航角一阶互补滤波
	 static int flag=0;
	 if(ms10>300&&(fabs(Att_Angle->rol)>5||fabs(Att_Angle->pit)>5)) flag=1;//倾角较大时
	    else flag=0;
		if(flag==0) Gyr_yaw=Att_Angle->yaw;
		//////////跳变处姿态校准///////////
	 if(ms10==300) gyr_yaw=Att_Angle->yaw;
	 gyr_yaw+=Gyr_rad->Z*0.42f+0.004f;
	 if(gyr_yaw>180) gyr_yaw=-180;
	 if(gyr_yaw<-180) gyr_yaw=180;
	 dif=fabs(Att_Angle->yaw-gyr_yaw);
	 if(dif<3&&fabs(Att_Angle->yaw)<135) gyr_yaw=Att_Angle->yaw;//检测磁力计是否发生异常跳变,是则使用陀螺仪数据，否则矫正陀螺仪&&当角度为-140到140间，使用校正后的陀螺仪数据
	 Att_Angle->yaw=gyr_yaw;
	 
	 if(flag==1) //倾角较大时姿态校准
		 {
				Gyr_yaw+=Gyr_rad->Z*0.45f+0.004f;
			 if(Gyr_yaw>180)  Gyr_yaw=-180;
	     if(Gyr_yaw<-180) Gyr_yaw=180;
				Att_Angle->yaw=Gyr_yaw;
		 }
	 if(Att_Angle->yaw>180) Att_Angle->yaw=180;
	 if(Att_Angle->yaw<-180) Att_Angle->yaw=-180;
	 
  //if(speedbase>400) Control_Yaw += Gyr_rad->Z*0.42f+0.004f ;   ///控制用的偏航角  
			//else Control_Yaw=0;
//	if(Control_Yaw>180)  Control_Yaw=-180;
//	if(Control_Yaw<-180) Control_Yaw=180;
	//Att_Angle->yaw = atan2(2.f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3)* 57.3f; // yaw
  Att_Angle->pit = -asin(2.f * (q1q3 - q0q2))* 57.3f;                                 // pitch(负号要注意) 
  Att_Angle->rol = atan2(2.f * q2q3 + 2.f * q0q1, q0q0 - q1q1 - q2q2 + q3q3)* 57.3f ; // roll
	
	ag=Acc_filt->Z*cos(Att_Angle->pit*DegtoRad )*cos(Att_Angle->rol*DegtoRad )+Acc_filt->Y*sin(Att_Angle->pit*DegtoRad )-Acc_filt->X*cos(Att_Angle->pit*DegtoRad )*sin(Att_Angle->rol*DegtoRad )-9.83284f;
	
	//RC_Filter(ag,&Z_G);
	//static int i;
	//static float ag_add;
	//if(i<10000){ i++;ag_add+=ag;}
	//if(i==10000) {ag_off=ag_add/10000;}
	agv+=ag*0.5f; //cm/s
	//agx+=agv*0.005f;
}



// 加速度计、地磁计、陀螺仪数据融合，更新四元数

/*

   [gx,gy,gz]为陀螺仪的测量值

   [ax,at,az]为加速度的测量值

   [mx,my,mz]为地磁计的测量值 

*/

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) 
{  
static	float q0 = 1, q1 = 0, q2 = 0, q3 = 0;     // quaternion elements representing the estimated orientation
static  float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
            float norm;               
            float hx, hy, hz, bx, bz;
            float vx, vy, vz, wx, wy, wz; 
            float ex, ey, ez;  

            // 定义一些辅助变量用于转换矩阵

            float q0q0 = q0*q0;  
            float q0q1 = q0*q1;  
            float q0q2 = q0*q2;  
            float q0q3 = q0*q3;  
            float q1q1 = q1*q1;  
            float q1q2 = q1*q2;  
            float q1q3 = q1*q3;  
            float q2q2 = q2*q2;  
            float q2q3 = q2*q3;  
            float q3q3 = q3*q3;  

            // 归一化加速度计和地磁计的度数 

            norm = sqrt(ax*ax + ay*ay + az*az);   
            ax = ax / norm;  
            ay = ay / norm;  
            az = az / norm;  

            norm = sqrt(mx*mx + my*my + mz*mz);   
            mx = mx / norm;  
            my = my / norm;  
            mz = mz / norm;  
   
            //将b系中的地磁计分量[mx,my,mz]转换到n系,得到[hx,hy,hz]  
            hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);  
            hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);  
            hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);        

            //得到n系中的地磁向量的真实值[bx,bz,by],其中by=0   
            bx = sqrt((hx*hx) + (hy*hy));  
            bz = hz;     

            //n系中的地磁向量[bx，by,bz]转换到b系中，得到[wx,wy,wz]
            wx = 2*bx*(0.5f - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);  
            wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);  
            wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5f - q1q1 - q2q2);                        

            //n系中重力加速度[0,0,1]转换到b系中得到三个分量[vx,vy,vz]        
            vx = 2*(q1q3 - q0q2);  
            vy = 2*(q0q1 + q2q3);  
            vz = q0q0 - q1q1 - q2q2 + q3q3;    

            //计算[wx,wy,wz] X [mx,my,mz],[ax,at,az] X [vx,vy,vz]，得到两个误差后求和
            ex = (ay*vz - az*vy) + (my*wz - mz*wy);  
            ey = (az*vx - ax*vz) + (mz*wx - mx*wz);  
            ez = (ax*vy - ay*vx) + (mx*wy - my*wx);  

            //PI控制器中的积分部分
            exInt = exInt + ex*Ki;  
            eyInt = eyInt + ey*Ki;  
            ezInt = ezInt + ez*Ki;  

            //误差经过PI控制器后输出,然后补偿到角速度的三个分量，Kp、Ki是需要调节的参数

            gx = gx + Kp*ex + exInt;  
            gy = gy + Kp*ey + eyInt;  
            gz = gz + Kp*ez + ezInt;               

            //一阶龙格库塔法更新四元数  

            q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;  
            q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;  
            q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;  
            q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;    
      
            // 归一化四元数
            norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);  
            q0 = q0 / norm;  
            q1 = q1 / norm;  
            q2 = q2 / norm; 
            q3 = q3 / norm;  
						Att_Angle1.yaw = atan2(2.f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3)* 57.3f; // yaw
						Att_Angle1.pit = -asin(2.f * (q1q3 - q0q2))* 57.3f;                                 // pitch(负号要注意) 
						Att_Angle1.rol = atan2(2.f * q2q3 + 2.f * q0q1, q0q0 - q1q1 - q2q2 + q3q3)* 57.3f ; // roll

}


/*********************************************************************************************************
*函  数：void IMU_OFFSETS(void)
*功　能：陀螺仪和加速度计校准，并将校准后的数据存入AT24C02芯片中
*参  数：无
*返回值：无
*备  注：对陀螺仪使用均值校准，加速度计使用六面校准法校准
**********************************************************************************************************/	
void IMU_OFFSETS(void)
{
	imu_off=0;
	static int i=0,gyr_flag=1,acc_flag=1;
	while(imu_off_flag<9)
	{
	
		if(PPM_Databuf[4]>1800&&PPM_Databuf[4]<2200) imu_off_flag++;
		
			if(imu_off_flag==1&&gyr_flag)
			{
				GPIO_SetBits(GPIOE,GPIO_Pin_1);
				for(i=0;i<1000;i++)
				{
					IMU();
					gyr_add.X+=Gyr_rad.X;
					gyr_add.Y+=Gyr_rad.Y;
					gyr_add.Z+=Gyr_rad.Z;
				}
					gyr_off.X=gyr_add.X/1000.0f;
					gyr_off.Y=gyr_add.Y/1000.0f;
					gyr_off.Z=gyr_add.Z/1000.0f;
					GPIO_ResetBits(GPIOE,GPIO_Pin_1);
					gyr_flag=0;
				}
		if(imu_off_flag>=2&&imu_off_flag<=7&&imu_off_flag>acc_flag)
		{
			acc_flag=imu_off_flag;
			GPIO_SetBits(GPIOE,GPIO_Pin_1);
			for(i=0;i<1000;i++)
			{
				IMU();
				acc_add.X+=Acc_filt.X;
				acc_add.Y+=Acc_filt.Y;
				acc_add.Z+=Acc_filt.Z;
			}
				acc_sample[imu_off_flag-2].X=acc_add.X/1000.0f;
				acc_sample[imu_off_flag-2].Y=acc_add.Y/1000.0f;
				acc_sample[imu_off_flag-2].Z=acc_add.Z/1000.0f;
				acc_add.X=acc_add.Y=acc_add.Z=0;
				GPIO_ResetBits(GPIOE,GPIO_Pin_1);
		}
		if(imu_off_flag==8)
			{
				Calibrate_accel(acc_sample,&acc_off,&acc_scale);
				Gyr_Buffer[0]=gyr_off.X;
				Gyr_Buffer[1]=gyr_off.Y;
				Gyr_Buffer[2]=gyr_off.Z;
				Acc_Buffer[0]=acc_off.X;
				Acc_Buffer[1]=acc_off.Y;
				Acc_Buffer[2]=acc_off.Z;
				Acc_Buffer[3]=acc_scale.X;
				Acc_Buffer[4]=acc_scale.Y;
				Acc_Buffer[5]=acc_scale.Z;
				imu_off=1;
				write_AT24C02(0,Gyr_Buffer,12);
				delay_ms(10);
				write_AT24C02(20,Acc_Buffer,24);
			}
	}
}


/*********************************************************************************************************
*函  数：void IMU_OFF_Read(void)
*功　能：读校准后AT24C02芯片中的校准数据
*参  数：无
*返回值：无
*备  注：无
**********************************************************************************************************/	
void IMU_OFF_Read(void)
{
	if(read_AT24C02(0,0,0)==0xaa) //判断是否已校准
			read_AT24C02(0,Gyr_Off,12);	
	if(read_AT24C02(20,0,0)==0xaa) //判断是否已校准
		{
				read_AT24C02(20,Acc_Off,24);	
		}	
	if(read_AT24C02(13,0,0)==0xaa) //判断是否已校准
			read_AT24C02(13,Mag_off,6);	
	//gyr_off.X=Gyr_Off[0];
	gyr_off.X=-0.012196f;
	gyr_off.Y=Gyr_Off[1];
	gyr_off.Z=Gyr_Off[2];
	acc_off.X=Acc_Off[0];
	acc_off.Y=Acc_Off[1];
	acc_off.Z=Acc_Off[2];
	acc_scale.X=Acc_Off[3];
	acc_scale.Y=Acc_Off[4];
	acc_scale.Z=Acc_Off[5];
	Mag_Off.X=Mag_off[0]; 
	Mag_Off.Y=Mag_off[1]; 
	Mag_Off.Z=Mag_off[2]; 
}
